﻿#include <scene_query.h>
#include <pi_lib.h>
#include <pi_mesh.h>
#include <opcode.h>
#include <new>


/* 平截头与 aabb 相交测试 */
PiBool frustum2aabb(PiAABBBox* aabb, void* params)
{
	PiFrustum* frustum = (PiFrustum*)params;
	return pi_frustum_is_aabb_visible(frustum, aabb);
}

/* 立方体与 aabb 相交测试 */
PiBool cube2aabb(PiAABBBox* aabb, void* params)
{
	cube2aabbParams* ps = (cube2aabbParams*)params;
	PiVector3 minCube = ps->minPt;
	PiVector3 maxCube = ps->maxPt;
	PiVector3 minBox = aabb->minPt;
	PiVector3 maxBox = aabb->maxPt;

	if (minCube.x > maxBox.x || maxCube.x < minBox.x)
		return FALSE;
	if (minCube.y > maxBox.y || maxCube.y < minBox.y)
		return FALSE;
	if (minCube.z > maxBox.z || maxCube.z < minBox.z)
		return FALSE;

	return TRUE;
}

/*aabb与aabb碰撞检测*/
PiBool aabb2aabb(PiAABBBox* aabb, void* params)
{
	PiAABBBox* box =(PiAABBBox* ) params;
	return pi_aabb_is_overlapped(aabb, box);
}

/*平面与aabb相交测试*/
PiBool plane2aabb(PiAABBBox* aabb, void* params)
{
	PiPlane* plane = (PiPlane*) params;
	PiVector3* min = &aabb->minPt;
	PiVector3* max = &aabb->maxPt;
	float d;
	float d2;

	d = pi_plane_dot(plane, min->x, min->y, min->z);

	d2 = pi_plane_dot(plane, min->x, min->y, max->z);
	if ((d2 <= 0.0f && d >= 0.0f) || (d2 >= 0.0f && d <= 0.0f))
	{
		return TRUE;
	}
	d2 = pi_plane_dot(plane, min->x, max->y, min->z);
	if ((d2 <= 0.0f && d >= 0.0f) || (d2 >= 0.0f && d <= 0.0f))
	{
		return TRUE;
	}
	d2 = pi_plane_dot(plane, min->x, max->y, max->z);
	if ((d2 <= 0.0f && d >= 0.0f) || (d2 >= 0.0f && d <= 0.0f))
	{
		return TRUE;
	}
	d2 = pi_plane_dot(plane, min->x, min->y, min->z);
	if ((d2 <= 0.0f && d >= 0.0f) || (d2 >= 0.0f && d <= 0.0f))
	{
		return TRUE;
	}
	d2 = pi_plane_dot(plane, max->x, min->y, max->z);
	if ((d2 <= 0.0f && d >= 0.0f) || (d2 >= 0.0f && d <= 0.0f))
	{
		return TRUE;
	}
	d2 = pi_plane_dot(plane, max->x, max->y, min->z);
	if ((d2 <= 0.0f && d >= 0.0f) || (d2 >= 0.0f && d <= 0.0f))
	{
		return TRUE;
	}
	d2 = pi_plane_dot(plane, max->x, max->y, max->z);
	if ((d2 <= 0.0f && d >= 0.0f) || (d2 >= 0.0f && d <= 0.0f))
	{
		return TRUE;
	}

	return FALSE;
}

/*球体与aabb相交测试*/
PiBool sphere2aabb(PiAABBBox* aabb, void* params)
{
	PiVector3 nearPt;
	PiVector3 centerPt;
	float nearX;
	float nearY;
	float nearZ;
	sphere2aabbParams* ps = (sphere2aabbParams*)params;
	float centerX = ps->x;
	float centerY = ps->y;
	float centerZ = ps->z;

	PiVector3* min = &aabb->minPt;
	PiVector3* max = &aabb->maxPt;
	//查找距离圆心最近的点
	if (centerX < min->x)
	{
		nearX = min->x;
	}
	else if (centerX > max->x)
	{
		nearX = max->x;
	}
	else
	{
		nearX = centerX;
	}

	if (centerY < min->y)
	{
		nearY = min->y;
	}
	else if (centerY > max->y)
	{
		nearY = max->y;
	}
	else
	{
		nearY = centerY;
	}

	if (centerZ < min->z)
	{
		nearZ = min->z;
	}
	else if (centerZ > max->z)
	{
		nearZ = max->z;
	}
	else
	{
		nearZ = centerZ;
	}

	pi_vec3_set(&nearPt, nearX, nearY, nearZ);
	pi_vec3_set(&centerPt, centerX, centerY, centerZ);
	if (pi_vec3_distance_square(&nearPt, &centerPt) <= ps->radius*ps->radius)
	{
		return TRUE;
	}
	else
	{
		return FALSE;
	}
}

/* 直线与 aabb 相交测试 */
PiBool line2aabb(PiAABBBox* aabb, void* params)
{
	line2aabbParams* ps = (line2aabbParams*)params;
	float x = ps->sx; // start x
	float y = ps->sy; // start y
	float z = ps->sz; // start z
	float x2 = ps->ex; // end x
	float y2 = ps->ey; // end y
	float z2 = ps->ez; // end z
	float dx, dy, dz;
	float t, hx, hy, hz;
	PiVector3 startPt, endPt;
	PiVector3* min = &aabb->minPt;
	PiVector3* max = &aabb->maxPt;
	
	pi_vec3_set(&startPt, x, y, z);
	pi_vec3_set(&endPt, x2, y2, z2);

	// 快速判断线段是否完全在 aabb 外
	if ((x < min->x && x2 < min->x) || 
		(x > max->x && x2 > max->x)	|| 
		(y < min->y && y2 < min->y)	||
		(y > max->y && y2 > max->y)	|| 
		(z < min->z && z2 < min->z)	|| 
		(z > max->z && z2 > max->z))
		return FALSE;

	// 快速判断线段是否至少一端在 aabb 内
	if (pi_aabb_is_contian_point(aabb, &startPt) || 
		pi_aabb_is_contian_point(aabb, &endPt))
		return TRUE;

	// 完整判断思路：
	// 依次测试线段和三对面每一对中最近的那个面是否相交，共判断三次
	dx = x2 - x;
	dy = y2 - y;
	dz = z2 - z;

	// yz 面
	if (x < min->x && dx > 0)
	{
		t = (min->x - x) / dx;
		hy = t * dy + y;
		hz = t * dz + z;
		if (hy > min->y && hy < max->y && hz > min->z && hz < max->z)
			return TRUE;
	}
	else if (x > max->x && dx < 0)
	{
		t = (max->x - x) / dx;
		hy = t * dy + y;
		hz = t * dz + z;
		if (hy > min->y && hy < max->y && hz > min->z && hz < max->z)
			return TRUE;
	}

	// xz 面
	if (y < min->y && dy > 0)
	{
		t = (min->y - y) / dy;
		hx = t * dx + x;
		hz = t * dz + z;
		if (hx > min->x && hx < max->x && hz > min->z && hz < max->z)
			return TRUE;
	}
	else if (y > max->y && dy < 0)
	{
		t = (max->y - y) / dy;
		hx = t * dx + x;
		hz = t * dz + z;
		if (hx > min->x && hx < max->x && hz > min->z && hz < max->z)
			return TRUE;
	}

	// xy 面
	if (z < min->z && dz > 0)
	{
		t = (min->z - z) / dz;
		hx = t * dx + x;
		hy = t * dy + y;
		if (hx > min->x && hx < max->x && hy > min->y && hy < max->y)
			return TRUE;
	}
	else if (z > max->z && dz < 0)
	{
		t = (max->z - z) / dz;
		hx = t * dx + x;
		hy = t * dy + y;
		if (hx > min->x && hx < max->x && hy > min->y && hy < max->y)
			return TRUE;
	}

	return FALSE;
}

/* 创建给定场景物体的碰撞模型 */
static PiBool prepare_collision_mode(S3dSceneObj* obj)
{
	uint i, numIndices, numVertices, numSubMesh;
	void* vb, *ib;
	EGeometryType gType;
	EIndexType iType;
	PiMesh* mesh = (PiMesh*)obj->attachment;
	Opcode::OPCODECREATE opcc;
	Opcode::MeshInterface* mi;
	Opcode::Model** models = (Opcode::Model**)obj->collisionModel;
	Opcode::MeshInterfaceType;
	if (obj->attachment == NULL)
		return FALSE;

	// 必要时需要重建碰撞模型
	if (models == NULL)
	{
		numSubMesh = pi_mesh_get_numsubmesh((PiMesh*)mesh);
		models = (Opcode::Model**)pi_malloc0(numSubMesh * sizeof(Opcode::Model*));

		for (i = 0; i < numSubMesh; i++)
		{
			// 仅支持三角形列表
			vb = pi_mesh_get_pos(mesh, i, &gType, &numVertices);
			PI_ASSERT(gType == EGOT_TRIANGLE_LIST, L"only triangle list supported");

			// 仅支持 32 位索引
			ib = pi_mesh_get_index(mesh, 0, &iType, &numIndices);
			PI_ASSERT(numIndices % 3 == 0 && iType == EINDEX_32BIT, L"only bit32 index supported");

			mi = (Opcode::MeshInterface*)pi_malloc0(sizeof(Opcode::MeshInterface));
			mi = new(mi)Opcode::MeshInterface();
			mi->SetNbTriangles(numIndices / 3);
			mi->SetNbVertices(numVertices);
			mi->SetPointers((IceMaths::IndexedTriangle*)ib, (IceMaths::Point*)vb);

			opcc.mIMesh = mi;
			opcc.mSettings.mRules = Opcode::SPLIT_GEOM_CENTER | Opcode::SPLIT_SPLATTER_POINTS;
			opcc.mSettings.mLimit = 1;
			opcc.mNoLeaf = true;
			opcc.mQuantized = false;

			models[i] = (Opcode::Model*)pi_malloc0(sizeof(Opcode::Model));
			models[i] = new(models[i])Opcode::Model();
			models[i]->Build(opcc);
		}

		obj->collisionModel = (void**)models;
	}

	return TRUE;
}

/* 销毁碰撞模型 */
void destroy_collision_mode(S3dSceneObj* obj)
{
	int32 i, numSubMesh;

	if (obj->collisionModel == NULL)
		return;

	numSubMesh = pi_mesh_get_numsubmesh((PiMesh*)obj->collisionModel);
	for (i = 0; i < numSubMesh; i++)
	{
		((Opcode::Model*)obj->collisionModel[i])->Model::~Model();
		pi_free(obj->collisionModel[i]);
	}
	
	pi_free(obj->collisionModel);
	obj->collisionModel = NULL;
}

/* 将 PiMatrix4 转换成 Ice::Matrix4x4 */
#define PIMAT2ICEMAT(piMat, iceMat) \
	iceMat.m[0][0] = piMat.m[0][0]; \
	iceMat.m[0][1] = piMat.m[1][0]; \
	iceMat.m[0][2] = piMat.m[2][0]; \
	iceMat.m[0][3] = piMat.m[3][0]; \
	iceMat.m[1][0] = piMat.m[0][1]; \
	iceMat.m[1][1] = piMat.m[1][1]; \
	iceMat.m[1][2] = piMat.m[2][1]; \
	iceMat.m[1][3] = piMat.m[3][1]; \
	iceMat.m[2][0] = piMat.m[0][2]; \
	iceMat.m[2][1] = piMat.m[1][2]; \
	iceMat.m[2][2] = piMat.m[2][2]; \
	iceMat.m[2][3] = piMat.m[3][2]; \
	iceMat.m[3][0] = piMat.m[0][3]; \
	iceMat.m[3][1] = piMat.m[1][3]; \
	iceMat.m[3][2] = piMat.m[2][3]; \
	iceMat.m[3][3] = piMat.m[3][3]

/* 检测两个物体是否相交 */
PiBool obj2obj_collision_check(S3dSceneObj* oa, S3dSceneObj* ob)
{
	int32 i, j, numA, numB;
	Opcode::AABBTreeCollider collider;
	Opcode::BVTCache cache;
	IceMaths::Matrix4x4 ma, mb;
	bool noErr = false;

	if (oa == ob)
		return FALSE;

	prepare_collision_mode(oa);
	prepare_collision_mode(ob);

	PIMAT2ICEMAT(oa->worldMatrix, ma);
	PIMAT2ICEMAT(ob->worldMatrix, mb);

	// 执行碰撞检测
	collider.SetFirstContact(true);
	collider.SetFullBoxBoxTest(false);
	collider.SetFullPrimBoxTest(false);
	collider.SetTemporalCoherence(false);

	numA = pi_mesh_get_numsubmesh((PiMesh*)oa->attachment);
	numB = pi_mesh_get_numsubmesh((PiMesh*)ob->attachment);
	for (i = 0; i < numA; i++)
	{
		for (j = 0; j < numB; j++)
		{
			cache.Model0 = (const Opcode::Model*)oa->collisionModel[i];
			cache.Model1 = (const Opcode::Model*)ob->collisionModel[j];

			PI_ASSERT(collider.ValidateSettings() == 0, L"collider is not ready yet");
			noErr = collider.Collide(cache, &ma, &mb);
			
			if (noErr && collider.GetContactStatus() && collider.GetNbPairs() > 0)
				return TRUE;
		}
	}

	return FALSE;
}

/**
* 检测射线与给定物体是否相交
* closetOnly为true时只取相交最近的点
*/
void PI_API s3d_collision_rayDist(S3dScene* scene, PiVector* srcList, PiVector* dstList, PiBytes* distanceList, float sx, float sy, float sz, float ex, float ey, float ez, PiBool forDistance)
{
	uint32 i, j, k, numSubmesh, num;
	PiBool noErr, collided;
	float dx, dy, dz, dist, minDist, maxDist;
	S3dSceneObj* obj;
	S3dOBJ ob;
	Opcode::RayCollider collider;
	IceMaths::Matrix4x4 m;
	IceMaths::Ray ray;
	Opcode::CollisionFaces cFace;

	num = pi_vector_size(srcList);
	if (num == 0)
		return;

	dx = ex - sx;
	dy = ey - sy;
	dz = ez - sz;
	dist = pi_math_sqrt(dx*dx + dy*dy + dz*dz);

	collider.SetFirstContact(forDistance == 0 ? true : false);
	collider.SetCulling(false);
	collider.SetClosestHit(false);
	collider.SetMaxDist(dist);
	collider.SetDestination(&cFace);
	collider.SetTemporalCoherence(false);

	ray.mOrig.Set(sx, sy, sz);
	ray.mDir.Set(dx/dist, dy/dist, dz/dist);

	pi_vector_clear(dstList, FALSE);
	for (i = 0; i < num; i++)
	{
		ob = (S3dOBJ)pi_vector_get(srcList, i);
		obj = OBJ(ob);
		prepare_collision_mode(obj);
		PIMAT2ICEMAT(obj->worldMatrix, m);
		numSubmesh = pi_mesh_get_numsubmesh((PiMesh*)obj->attachment);
		collided = false;
		for (j = 0; j < numSubmesh; j++)
		{
			noErr = collider.Collide(ray, *((Opcode::Model*)obj->collisionModel[j]), &m);
			collided = noErr && collider.GetContactStatus() && collider.GetNbIntersections();
			if (collided)
			{
				if (forDistance && distanceList != NULL)
				{
					minDist = dist;
					maxDist = 0;
					for (k = 0; k < cFace.GetNbFaces(); k++)
					{
						if (cFace.GetFaces()[k].mDistance < minDist)
							minDist = cFace.GetFaces()[k].mDistance;

						if (cFace.GetFaces()[k].mDistance > maxDist)
							maxDist = cFace.GetFaces()[k].mDistance;
					}

					pi_bytes_write_double(distanceList, minDist);
					pi_bytes_write_double(distanceList, maxDist);
				}

				pi_vector_push(dstList, (void*)ob);
				break;
			}
		}
	}

	return;
}

/*检测扇形与给定物体是否相交*/
void PI_API s3d_collision_sector2objs(S3dScene* scene, PiVector* srcList, PiVector* dstList, float radius, float radians, PiMatrix4* worldMatrix, int number)
{
	PiBool s1, s2;
	int32 i, j;
	int32 num, numSubmesh;
	float tempRadians, stepRadian;
	int32 vNum = (number+2)*3;
	int32 iNum = number*3;
	float* vertex = pi_new0(float, vNum);//顶点数组
	int* index = pi_new0(int, iNum);  //索引数组
	PiMatrix4 sMatrix = *worldMatrix;	//扇形的世界变换矩阵
	Opcode::OPCODECREATE opcc;
	Opcode::MeshInterface* mi;
	Opcode::Model* models;
	Opcode::MeshInterfaceType iType = Opcode::MESH_TRIANGLE;
	Opcode::AABBTreeCollider collider;
	Opcode::BVTCache cache;
	IceMaths::Matrix4x4 ma, mb;
	S3dSceneObj* obj;
	S3dOBJ ob;
	bool noErr = false;
	//构造扇形的碰撞模型
	vertex[0] = 0.0f;
	vertex[1] = 0.0f;
	vertex[2] = 0.0f;
	stepRadian = radians*2/number;
	for (i =0; i <= number; i++)
	{
		tempRadians = (float)(PI_PI/2)-radians+stepRadian*i; 
		vertex[(i+1)*3] = radius*pi_math_cos(tempRadians);
		vertex[(i+1)*3+1] = 0.0f;
		vertex[(i+1)*3+2] = radius*pi_math_sin(tempRadians);
	}
	i = 0;
	while (i < number)
	{
		index[i*3] = 0;
		index[i*3+1] = i+1;
		index[i*3+2] = i+2;
		i++;
	}

	mi = (Opcode::MeshInterface*)pi_malloc0(sizeof(Opcode::MeshInterface));
	mi = new(mi)Opcode::MeshInterface();
	mi->SetNbTriangles(number);
	mi->SetNbVertices(number+2);
	mi->SetInterfaceType(iType);
	mi->SetPointers((IceMaths::IndexedTriangle*)index, (IceMaths::Point*)vertex);

	opcc.mIMesh = mi;
	opcc.mSettings.mRules = Opcode::SPLIT_GEOM_CENTER | Opcode::SPLIT_SPLATTER_POINTS;
	opcc.mSettings.mLimit = 1;
	opcc.mNoLeaf = true;
	opcc.mQuantized = false;

	models = (Opcode::Model*)pi_malloc0(sizeof(Opcode::Model));
	models = new(models)Opcode::Model();
	models->Build(opcc);
	//碰撞检测
	collider.SetFirstContact(false);
	collider.SetFullBoxBoxTest(false);
	collider.SetFullPrimBoxTest(false);
	collider.SetTemporalCoherence(false);
	num = pi_vector_size(srcList);
	if (num == 0)
	{
		pi_free(vertex);
		pi_free(index);
		return;
	}
	PIMAT2ICEMAT(sMatrix, mb);
	pi_vector_clear(dstList, FALSE);
	for (i = 0; i < num; i++)
	{
		ob = (S3dOBJ)pi_vector_get(srcList, i);
		obj = OBJ(ob);
		prepare_collision_mode(obj);
		PIMAT2ICEMAT(obj->worldMatrix, ma);
		numSubmesh = pi_mesh_get_numsubmesh((PiMesh*)obj->attachment);
		for (j = 0; j < numSubmesh; j++)
		{
			cache.Model0 = (const Opcode::Model*)obj->collisionModel[j];
			cache.Model1 = models;

			PI_ASSERT(collider.ValidateSettings() == 0, L"collider is not ready yet");
			noErr = collider.Collide(cache, &ma, &mb);
			s1 = collider.GetContactStatus();
			s2 = collider.GetNbPairs();
			if (noErr && collider.GetContactStatus() && collider.GetNbPairs() > 0)
			{
				pi_vector_push(dstList, (void*)ob);
				break;
			}
		}
	}

	pi_free(vertex);
	pi_free(index);
}

/*检测圆柱与给定物体是否相交*/
void PI_API s3d_collision_cylinder2objs(S3dScene* scene, PiVector* srcList, PiVector* dstList, float radius, float height, PiMatrix4* worldMatrix, int number)
{
	float radians;		//每次增长的弧度
	int32 vNum = (4*number)*3;
	int32 iNum = number*6;
	float* vertex = pi_new0(float, vNum);//顶点数组
	int* index = pi_new0(int, iNum);  //索引数组

	PiBool s1, s2;
	int32 i, j;
	int32 num, numSubmesh;
	float tempRadians;
	PiMatrix4 sMatrix = *worldMatrix;	//扇形的世界变换矩阵
	Opcode::OPCODECREATE opcc;
	Opcode::MeshInterface* mi;
	Opcode::Model* models;
	Opcode::MeshInterfaceType iType = Opcode::MESH_TRIANGLE;
	Opcode::AABBTreeCollider collider;
	Opcode::BVTCache cache;
	IceMaths::Matrix4x4 ma, mb;
	S3dSceneObj* obj;
	S3dOBJ ob;
	bool noErr = false;

	radians = (float)PI_PI / number;
	//构造圆柱的定点数组
	for (i = 0; i < number; i++)
	{
		tempRadians = i*radians; 
		vertex[i*3] = radius*pi_math_cos(tempRadians);
		vertex[i*3+1] = -height;
		vertex[i*3+2] = radius*pi_math_sin(tempRadians);
		
		vertex[i*3+number] = -radius*pi_math_cos(tempRadians);
		vertex[i*3+1+number] = -height;
		vertex[i*3+2] = -radius*pi_math_sin(tempRadians);

		vertex[i*3+number*2] = radius*pi_math_cos(tempRadians);
		vertex[i*3+1+number*2] = height;
		vertex[i*3+2] = radius*pi_math_sin(tempRadians);

		vertex[i*3+number*3] = -radius*pi_math_cos(tempRadians);
		vertex[i*3+1+number*3] = height;
		vertex[i*3+2+number*3] = -radius*pi_math_sin(tempRadians);
	}

	//计算索引数组
	i = 0;
	while (i < number) 
	{
		index[i*3] = i;
		index[i*3+1] = i+number*3;
		index[i*3+2] = i+number*2;
		i++;
		index[i*3] = i;
		index[i*3+1] = i+number;
		index[i*3+2] = i+number*3;
		i++;
	}

	//模型转换
	mi = (Opcode::MeshInterface*)pi_malloc0(sizeof(Opcode::MeshInterface));
	mi = new(mi)Opcode::MeshInterface();
	mi->SetNbTriangles(number*2);
	mi->SetNbVertices(number*4);
	mi->SetInterfaceType(iType);
	mi->SetPointers((IceMaths::IndexedTriangle*)index, (IceMaths::Point*)vertex);

	opcc.mIMesh = mi;
	opcc.mSettings.mRules = Opcode::SPLIT_GEOM_CENTER | Opcode::SPLIT_SPLATTER_POINTS;
	opcc.mSettings.mLimit = 1;
	opcc.mNoLeaf = true;
	opcc.mQuantized = false;

	models = (Opcode::Model*)pi_malloc0(sizeof(Opcode::Model));
	models = new(models)Opcode::Model();
	models->Build(opcc);
	//碰撞检测
	collider.SetFirstContact(false);
	collider.SetFullBoxBoxTest(false);
	collider.SetFullPrimBoxTest(false);
	collider.SetTemporalCoherence(false);
	num = pi_vector_size(srcList);
	if (num == 0)
	{
		pi_free(vertex);
		pi_free(index);
		return;
	}
	PIMAT2ICEMAT(sMatrix, mb);
	pi_vector_clear(dstList, FALSE);
	for (i = 0; i < num; i++)
	{
		ob = (S3dOBJ)pi_vector_get(srcList, i);
		obj = OBJ(ob);
		prepare_collision_mode(obj);
		PIMAT2ICEMAT(obj->worldMatrix, ma);
		numSubmesh = pi_mesh_get_numsubmesh((PiMesh*)obj->attachment);
		for (j = 0; j < numSubmesh; j++)
		{
			cache.Model0 = (const Opcode::Model*)obj->collisionModel[j];
			cache.Model1 = models;

			PI_ASSERT(collider.ValidateSettings() == 0, L"collider is not ready yet");
			noErr = collider.Collide(cache, &ma, &mb);
			s1 = collider.GetContactStatus();
			s2 = collider.GetNbPairs();
			if (noErr && collider.GetContactStatus() && collider.GetNbPairs() > 0)
			{
				pi_vector_push(dstList, (void*)ob);
				break;
			}
		}
	}
	pi_free(vertex);
	pi_free(index);
}

/*检测圆锥与给定物体是否相交*/
void PI_API s3d_collision_cone2objs(S3dScene* scene, PiVector* srcList, PiVector* dstList, float radius, float height, PiMatrix4* worldMatrix, int number)
{
	float radians;		//每次增长的弧度
	int32 vNum = (2*number+1)*3;
	int32 iNum = number*3;
	float* vertex = pi_new0(float, vNum);//顶点数组
	int* index = pi_new0(int, iNum);  //索引数组

	PiBool s1, s2;
	int32 i, j;
	int32 num, numSubmesh;
	float tempRadians;
	PiMatrix4 sMatrix = *worldMatrix;	//扇形的世界变换矩阵
	Opcode::OPCODECREATE opcc;
	Opcode::MeshInterface* mi;
	Opcode::Model* models;
	Opcode::MeshInterfaceType iType = Opcode::MESH_TRIANGLE;
	Opcode::AABBTreeCollider collider;
	Opcode::BVTCache cache;
	IceMaths::Matrix4x4 ma, mb;
	S3dSceneObj* obj;
	S3dOBJ ob;
	bool noErr = false;

	radians = (float)PI_PI / number;
	//构造扇形的碰撞模型
	vertex[0] = 0.0f;
	vertex[1] = height;
	vertex[2] = 0.0f;
	for (i =0; i < number; i++)
	{
		tempRadians = i*radians; 
		vertex[(i+1)*3] = radius*pi_math_cos(tempRadians);
		vertex[(i+1)*3+1] = 0.0f;
		vertex[(i+1)*3+2] = radius*pi_math_sin(tempRadians);

		vertex[(i+1)*3+number] = -radius*pi_math_cos(tempRadians);
		vertex[(i+1)*3+1+number] = 0.0f;
		vertex[(i+1)*3+2+number] = -radius*pi_math_sin(tempRadians);

	}
	i = 0;
	while (i < number)
	{
		index[i*3] = i+1+number;
		index[i*3+1] = i+1;
		index[i*3+2] = 0;
		i++;
	}
	//模型转换
	mi = (Opcode::MeshInterface*)pi_malloc0(sizeof(Opcode::MeshInterface));
	mi = new(mi)Opcode::MeshInterface();
	mi->SetNbTriangles(number);
	mi->SetNbVertices(2*number+1);
	mi->SetInterfaceType(iType);
	mi->SetPointers((IceMaths::IndexedTriangle*)index, (IceMaths::Point*)vertex);

	opcc.mIMesh = mi;
	opcc.mSettings.mRules = Opcode::SPLIT_GEOM_CENTER | Opcode::SPLIT_SPLATTER_POINTS;
	opcc.mSettings.mLimit = 1;
	opcc.mNoLeaf = true;
	opcc.mQuantized = false;

	models = (Opcode::Model*)pi_malloc0(sizeof(Opcode::Model));
	models = new(models)Opcode::Model();
	models->Build(opcc);
	//碰撞检测
	collider.SetFirstContact(false);
	collider.SetFullBoxBoxTest(false);
	collider.SetFullPrimBoxTest(false);
	collider.SetTemporalCoherence(false);
	num = pi_vector_size(srcList);
	if (num == 0)
	{
		pi_free(vertex);
		pi_free(index);
		return;
	}
	PIMAT2ICEMAT(sMatrix, mb);
	pi_vector_clear(dstList, FALSE);
	for (i = 0; i < num; i++)
	{
		ob = (S3dOBJ)pi_vector_get(srcList, i);
		obj = OBJ(ob);
		prepare_collision_mode(obj);
		PIMAT2ICEMAT(obj->worldMatrix, ma);
		numSubmesh = pi_mesh_get_numsubmesh((PiMesh*)obj->attachment);
		for (j = 0; j < numSubmesh; j++)
		{
			cache.Model0 = (const Opcode::Model*)obj->collisionModel[j];
			cache.Model1 = models;

			PI_ASSERT(collider.ValidateSettings() == 0, L"collider is not ready yet");
			noErr = collider.Collide(cache, &ma, &mb);
			s1 = collider.GetContactStatus();
			s2 = collider.GetNbPairs();
			if (noErr && collider.GetContactStatus() && collider.GetNbPairs() > 0)
				pi_vector_push(dstList, (void*)ob);
				break;
		}
	}
	pi_free(vertex);
	pi_free(index);
}

/*检测球体与给定物体是否相交*/
void PI_API s3d_collision_sphere2objs(S3dScene* scene, PiVector* srcList, PiVector* dstList, float centerX, float centerY, float centerZ, float radius)
{
	int32 i, j, num, numSubmesh;
	PiBool noErr, collided;
	S3dSceneObj* obj;
	S3dOBJ ob;
	Opcode::SphereCollider collider;
	IceMaths::Matrix4x4 m;
	IceMaths::Matrix4x4 m2;
	m2.Identity();
	Opcode::SphereCache cache;

	IceMaths::Sphere sphere;
	IceMaths::Point center;
	center.Set(centerX, centerY, centerZ);

	collider.SetFirstContact(false);
	collider.SetTemporalCoherence(false);
	sphere.SetCenter(center);
	sphere.SetRadius(radius);

	num = pi_vector_size(srcList);
	if (num == 0)
		return;
	pi_vector_clear(dstList, FALSE);
	for (i = 0; i < num; i++)
	{
		ob = (S3dOBJ)pi_vector_get(srcList, i);
		obj = OBJ(ob);
		prepare_collision_mode(obj);
		PIMAT2ICEMAT(obj->worldMatrix, m);
		collided = false;

		numSubmesh = pi_mesh_get_numsubmesh((PiMesh*)obj->attachment);
		for (j = 0; j < numSubmesh; j++)
		{
			noErr = collider.Collide(cache, sphere, *((Opcode::Model*)obj->collisionModel[j]), &m2, &m);
			collided = noErr && collider.GetContactStatus();
			if (collided)
			{
				pi_vector_push(dstList, (void*)ob);
				break;
			}
		}
	}
}

/*检测长方体与给定物体是否相交*/
void PI_API s3d_collision_cube2objs(S3dScene* scene, PiVector* srcList, PiVector* dstList, float minX, float minY, float minZ, float maxX, float maxY, float maxZ)
{
	int32 i, j, num, numSubmesh;
	S3dOBJ ob;		//场景物体的句柄
	PiBool noErr, collided;
	S3dSceneObj* obj;
	Opcode::AABBCollider collider;
	Opcode::AABBCache cache;
	IceMaths::AABB box;

	IceMaths::Point minPt;
	IceMaths::Point maxPt;
	minPt.Set(minX, minY, minZ);
	maxPt.Set(maxX, maxY, maxZ);

	box.SetMinMax(minPt, maxPt);
	collider.SetFirstContact(false);
	collider.SetPrimitiveTests(true);
	
	collider.SetTemporalCoherence(false);
	num = pi_vector_size(srcList);
	if (num == 0)
		return;

	pi_vector_clear(dstList, FALSE);
	for (i = 0; i < num; i++)
	{
		ob = (S3dOBJ)pi_vector_get(srcList, i);
		obj = OBJ(ob);
		prepare_collision_mode(obj);
		collided = false;

		numSubmesh = pi_mesh_get_numsubmesh((PiMesh*)obj->attachment);
		for (j = 0; j < numSubmesh; j++)
		{
			noErr = collider.Collide(cache, box, *((Opcode::Model*)obj->collisionModel[j]));
			collided = noErr && collider.GetContactStatus();
			if (collided)
			{
				pi_vector_push(dstList, (void*)ob);
				break;
			}
		}
	}
}

/*检测平面与给定物体是否相交*/
void PI_API s3d_collision_plane2objs(S3dScene* scene, PiVector* srcList, PiVector* dstList, float x, float y, float z, float d)
{
	int32 i, j, num, numSubmesh;
	bool noErr, collided;
	S3dSceneObj* obj;
	S3dOBJ ob;
	IceMaths::Matrix4x4 m;
	IceMaths::Matrix4x4 m2;
	Opcode::PlanesCollider collider;
	Opcode::PlanesCache cache;
	IceMaths::Plane plane;

	m2.Identity();
	plane.Set(x, y, z, d);
	plane.Normalize();
	collider.SetFirstContact(false);
	collider.SetTemporalCoherence(false);
	num = pi_vector_size(srcList);
	if (num == 0)
		return;

	pi_vector_clear(dstList, FALSE);
	for (i = 0; i < num; i++)
	{
		ob = (S3dOBJ)pi_vector_get(srcList, i);
		obj = OBJ(ob);
		prepare_collision_mode(obj);
		PIMAT2ICEMAT(obj->worldMatrix, m);
		collided = false;

		numSubmesh = pi_mesh_get_numsubmesh((PiMesh*)obj->attachment);
		for (j = 0; j < numSubmesh; j++)
		{
			noErr = collider.Collide(cache, &plane, 1, *((Opcode::Model*)obj->collisionModel[j]), &m);
			collided = noErr && collider.GetContactStatus();
			if (collided)
			{
				pi_vector_push(dstList, (void*)ob);
				break;
			}
		}
	}
}